%% deduce_centrifugal_coriolis.m

%% 2-joint
% syms m111 m121 m221 m112 m122 m222 real;
% syms dQ1 dQ2 real;
% 
% m11 = m111 * dQ1 + m112 * dQ2;
% m12 = m121 * dQ1 + m122 * dQ2;
% m22 = m221 * dQ1 + m222 * dQ2;
% 
% dQ = [dQ1; dQ2];
% M = [m11, m12; m12, m22];
% m1 = [m111, m121; m121, m221];
% m2 = [m112, m122; m122, m222];
% N = [dQ' * m1 * dQ; dQ' * m2 * dQ];
% V = M * dQ - sym(0.5) * N;
% V = simplify(V);
% 
% CC = sym(zeros(2, 2, 2));
% C1 = sym(zeros(2, 2));
% C2 = sym(zeros(2, 2));
% 
% for i = 1:2
%     Q_ = zeros(2, 1);
%     Q_(i) = 1;
%     for j = 1:2
%         CC(j, i, i) = subs(V(j), {'dQ1', 'dQ2'}, {Q_(1), Q_(2)});
%     end
% end
% 
% for i = 1:1
%     for j = (i+1):2
%         Q_ = zeros(2, 1);
%         Q_(i) = 1;
%         Q_(j) = 1;
%         for k = 1:2
%             T_ = subs(V(k), {'dQ1', 'dQ2'}, {Q_(1), Q_(2)});
%             CC(k, i, j) = sym(0.5) * (T_ - CC(k, i, i) - CC(k, j, j));
%             % CC(k, i, j) = T_ - CC(k, i, i) - CC(k, j, j);
%         end
%     end
% end
% 
% for i = 1:2
%     for j  = 1:2
%         C1(i, j) = CC(1, i, j);
%         C2(i, j) = CC(2, i, j);
%     end
% end

%% 3-joint model
syms m111 m121 m131 m221 m231 m331 real;
syms m112 m122 m132 m222 m232 m332 real;
syms m113 m123 m133 m223 m233 m333 real;
syms dQ1 dQ2 dQ3 real;

m11 = m111 * dQ1 + m112 * dQ2 + m113 * dQ3;
m12 = m121 * dQ1 + m122 * dQ2 + m123 * dQ3;
m13 = m131 * dQ1 + m132 * dQ2 + m133 * dQ3;
m22 = m221 * dQ1 + m222 * dQ2 + m223 * dQ3;
m23 = m231 * dQ1 + m232 * dQ2 + m233 * dQ3;
m33 = m331 * dQ1 + m332 * dQ2 + m333 * dQ3;

dQ = [dQ1; dQ2; dQ3];
M = [m11, m12, m13; 
     m12, m22, m23;
	 m13, m23, m33];
m1 = [m111, m121, m131;
      m121, m221, m231;
	  m131, m231, m331];
m2 = [m112, m122, m132; 
      m122, m222, m232;
	  m132, m232, m332];
m3 = [m113, m123, m133;
      m123, m223, m233;
	  m133, m233, m333];

N = [dQ' * m1 * dQ; dQ' * m2 * dQ; dQ' * m3 * dQ];
V = M * dQ - sym(0.5) * N;

% CC's coeffs is 1
CC = sym(zeros(3, 3, 3));
C1 = sym(zeros(3, 3));
C2 = sym(zeros(3, 3));
C3 = sym(zeros(3, 3));

for i = 1:3
    Q_ = zeros(3, 1);
    Q_(i) = 1;
    for j = 1:3
        CC(j, i, i) = subs(V(j), {'dQ1', 'dQ2', 'dQ3'}, {Q_(1), Q_(2), Q_(3)});
    end
end

for i = 1:2
    for j = (i+1):3
        Q_ = zeros(3, 1);
        Q_(i) = 1;
        Q_(j) = 1;
        for k = 1:3
            T_ = subs(V(k), {'dQ1', 'dQ2', 'dQ3'}, {Q_(1), Q_(2), Q_(3)});
            % CC(k, i, j) = sym(0.5) * (T_ - CC(k, i, i) - CC(k, j, j));
            CC(k, i, j) = T_ - CC(k, i, i) - CC(k, j, j);
        end
    end
end

for i = 1:3
    for j  = 1:3
        C1(i, j) = CC(1, i, j);
        C2(i, j) = CC(2, i, j);
		C3(i, j) = CC(3, i, j);
    end
end

TC1 = dQ' * C1 * dQ;
TC2 = dQ' * C2 * dQ;
TC3 = dQ' * C3 * dQ;
TC = [CC(1, 1, 1) * dQ1 + CC(1, 1, 2) * dQ2 + CC(1, 1, 3) * dQ3, ...
      CC(1, 1, 2) * dQ1 + CC(1, 2, 2) * dQ2 + CC(1, 2, 3) * dQ3, ...
	  CC(1, 1, 3) * dQ1 + CC(1, 2, 3) * dQ2 + CC(1, 3, 3) * dQ3;
	  CC(2, 1, 1) * dQ1 + CC(2, 1, 2) * dQ2 + CC(2, 1, 3) * dQ3, ...
      CC(2, 1, 2) * dQ1 + CC(2, 2, 2) * dQ2 + CC(2, 2, 3) * dQ3, ...
	  CC(2, 1, 3) * dQ1 + CC(2, 2, 3) * dQ2 + CC(2, 3, 3) * dQ3;
	  CC(3, 1, 1) * dQ1 + CC(3, 1, 2) * dQ2 + CC(3, 1, 3) * dQ3, ...
      CC(3, 1, 2) * dQ1 + CC(3, 2, 2) * dQ2 + CC(3, 2, 3) * dQ3, ...
	  CC(3, 1, 3) * dQ1 + CC(3, 2, 3) * dQ2 + CC(3, 3, 3) * dQ3;];
TCQ = TC * dQ;

VV = eval(subs(V, ...
         {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
          'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
          'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
          'dQ1', 'dQ2', 'dQ3'}, ...
          {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
           0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
           1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
           2.0, 7.0, 11.0}));
		   
TT = eval(subs(TCQ, ...
         {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
          'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
          'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
          'dQ1', 'dQ2', 'dQ3'}, ...
          {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
           0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
           1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
           2.0, 7.0, 11.0}));

T1 = eval(subs(TC1, ...
         {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
          'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
          'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
          'dQ1', 'dQ2', 'dQ3'}, ...
          {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
           0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
           1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
           2.0, 7.0, 11.0}));

T2 = eval(subs(TC2, ...
         {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
          'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
          'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
          'dQ1', 'dQ2', 'dQ3'}, ...
          {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
           0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
           1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
           2.0, 7.0, 11.0}));

T3 = eval(subs(TC3, ...
         {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
          'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
          'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
          'dQ1', 'dQ2', 'dQ3'}, ...
          {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
           0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
           1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
           2.0, 7.0, 11.0}));
disp(['Coeff1 dQ*C*dQ error: ', num2str(sum(abs(VV - [T1, T2, T3]')))]);
disp(['Coeff1 MatC error: ', num2str(sum(abs(VV - TT)))]);

% CC's coeffs is 0.5
% CC = sym(zeros(3, 3, 3));
% C1 = sym(zeros(3, 3));
% C2 = sym(zeros(3, 3));
% C3 = sym(zeros(3, 3));
% 
% for i = 1:3
%     Q_ = zeros(3, 1);
%     Q_(i) = 1;
%     for j = 1:3
%         CC(j, i, i) = subs(V(j), {'dQ1', 'dQ2', 'dQ3'}, {Q_(1), Q_(2), Q_(3)});
%     end
% end
% 
% for i = 1:2
%     for j = (i+1):3
%         Q_ = zeros(3, 1);
%         Q_(i) = 1;
%         Q_(j) = 1;
%         for k = 1:3
%             T_ = subs(V(k), {'dQ1', 'dQ2', 'dQ3'}, {Q_(1), Q_(2), Q_(3)});
%             CC(k, i, j) = sym(0.5) * (T_ - CC(k, i, i) - CC(k, j, j));
%         end
%     end
% end
% 
% for i = 1:2
%     for j  = 1:2
%         C1(i, j) = CC(1, i, j);
%         C2(i, j) = CC(2, i, j);
% 		C3(i, j) = CC(3, i, j);
%     end
% end
% 
% TC1 = dQ' * C1 * dQ;
% TC2 = dQ' * C2 * dQ;
% TC3 = dQ' * C3 * dQ;
% 
% VV = eval(subs(V, ...
%          {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
%           'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
%           'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
%           'dQ1', 'dQ2', 'dQ3'}, ...
%           {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
%            0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
%            1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
%            2.0, 7.0, 11.0}));
% 
% T1 = eval(subs(TC1, ...
%          {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
%           'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
%           'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
%           'dQ1', 'dQ2', 'dQ3'}, ...
%           {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
%            0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
%            1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
%            2.0, 7.0, 11.0}));
% 
% T2 = eval(subs(TC2, ...
%          {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
%           'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
%           'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
%           'dQ1', 'dQ2', 'dQ3'}, ...
%           {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
%            0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
%            1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
%            2.0, 7.0, 11.0}));
% 
% T3 = eval(subs(TC3, ...
%          {'m111', 'm121', 'm131', 'm221', 'm231', 'm331', ...
%           'm112', 'm122', 'm132', 'm222', 'm232', 'm332', ...
%           'm113', 'm123', 'm133', 'm223', 'm233', 'm333', ...
%           'dQ1', 'dQ2', 'dQ3'}, ...
%           {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, ...
%            0.7, 0.8, 0.9, 1.0, 1.1, 1.2, ...
%            1.3, 1.4, 1.5, 1.6, 1.7, 1.8, ...
%            2.0, 7.0, 11.0}));
% disp(['Coeff0.5 error: ', num2str(sum(abs(VV - [T1, T2, T3]')))]);